function [cost_map] = GenerateCostMap(weather,aircraft,target,time,alt)
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%

t=floor((time-weather.t0)./weather.ts) +1;

if (t < 1) time=0; end
if (t > weather.tnum) time = weather.tf; end;

fname = sprintf('bin/[cost_map]_%s_%ds_%dm_%dms.mat',weather.source,time-mod(time,60),aircraft.z,aircraft.velocity);
fname = sprintf('bin/[cost_map]_%s_%ds_%dms.mat',weather.source,time-mod(time,60),aircraft.velocity);
fname = sprintf('bin/[cost_map]_%s_%ds_%dms_mesh.mat',weather.source,time-mod(time,60),aircraft.velocity);

if exist(fname) == 2
  load(fname);
  return;
end;

%2d approximation
k=floor((alt-weather.z0)/weather.zs) +1;

target_x = floor((target.x-weather.x0)/weather.xs) +1;
target_y = floor((target.y-weather.y0)/weather.ys) +1;

%--[ step 1 ]--%
%cost_map = Inf*ones(weather.xnum,weather.ynum);

accepted = nodeList();
accepted_front = nodeList();
considered = nodeList();
far = nodeList();

%for i = weather.x
	%for j = weather.y
	  %node = dlnode([i,j], inf);
	  %far.append(node);
	%end
%end
load new_weather;
far=new_weather;
far.resetValues();
%h = norm([weather.xs,weather.ys]);
%h = min([weather.xs,weather.ys]);
h = 15000;

PlotWavefront(target,accepted,accepted_front,considered,far,3)
pause(0.1);

%--[ step 2 ]--%
node = far.findAt([target.x,target.y]);
node.V = 0;

far.remove(node);
accepted_front.append(node);
node.Position
%cost_map(node.Position(1)+1,node.Position(2)+1) = 0;

PlotWavefront(target,accepted,accepted_front,considered,far,3)
pause(0.1);
%--[ step 3 ]--%

adjacent_points = far.findAdjacentTo(node.Position,h);

current = adjacent_points.getFirst();
while ~isempty(current)
	temp = current;
	current = current.Next;
	temp.V = CalcCost(temp.Position(1),temp.Position(2),node.Position(1),node.Position(2),weather,aircraft,time,alt,node.V);
%	cost_map(temp.Position(1)+1,temp.Position(2)+1) = temp.V;
	far.remove(temp);
	considered.append(temp);
end

PlotWavefront(target,accepted,accepted_front,considered,far,3)
pause(0.1);

handle = waitbar(0,'Caluclating Cost');

while considered.Size > 0
  %--[ step 4 ]--%
  node = considered.findMinV();

  %--[ step 5 ]--%
	considered.remove(node);
	accepted_front.appendInOrder(node);
	%FIXME clear out accepted but not accepted_front points

  %--[ step 6 ]--%
	adjacent_points = far.findAdjacentTo(node.Position,h);

	current = adjacent_points.getFirst();
	while ~isempty(current)
		temp = current;
		current = current.Next;
		%FIXME Calcualte Correct V
		temp.V = CalcCost(temp.Position(1),temp.Position(2),node.Position(1),node.Position(2),weather,aircraft,time,alt,node.V);
%		cost_map(temp.Position(1)+1,temp.Position(2)+1) = temp.V;
		far.remove(temp);
		considered.append(temp);
	end

	PlotWavefront(target,accepted,accepted_front,considered,far,3)
	pause(0.1);

  %--[ step 7 ]--%
	%FIXME Calcualte NF, Correct V
%	current = considered.getFirst();
%	while ~isempty(current)
%		%FIXME Calcualte Correct V
%		current.V = CalcCost(current.Position(1),current.Position(2),node.Position(1),node.Position(2),weather,aircraft,time,alt,node.V);
%		current = current.Next;
%	end

  %--[ step 8 ]--%
  %waitbar((accepted.Size+accepted_front.Size)/(weather.xnum*weather.ynum),handle);
  waitbar((accepted.Size+accepted_front.Size)/(far.Size+accepted_front.Size+accepted.Size+considered.Size),handle);
end

close(handle);
clear handle;

cost_map = accepted_front;  %FIXME -- accepted doesn't work yet
save(fname,'cost_map');
